﻿/* ***********************************************************************************************
 * Oak_Kinematic
 * Version: 1.0
 * Author:  BRyan Solero  
 * Email:  arindil@gmail.com
 * Copyright 2012.  All Rights Reserved.
 * 
 * ************************************************************************************************/
using System;

using Microsoft.Xna.Framework;
using OakEngine.AI.Steering;
namespace OakEngine.AI.Kinematic
{
    public class Oak_Kinematic
    {
        private Vector2 m_Position;
        private Vector2 m_Direction;
        private float m_fAngularVelocity;
        private float m_fRotation;
        public Vector2 Position { get { return m_Position; } set { m_Position = value; } }
        public Vector2 Direction { get { return m_Direction; } set { m_Direction = value; } }
        public float AngularVelocity { get { return m_fAngularVelocity; } set { m_fAngularVelocity = value; } }
        public float Rotation { get { return m_fRotation; } set { m_fRotation = value; } }

        public Oak_Kinematic()
        {
            m_Position = new Vector2();
            m_Direction = new Vector2();
            m_fRotation = 0.0f;
            m_fAngularVelocity = 0.0f;
        }

        public Oak_Kinematic(Vector2 pos, Vector2 dir)
        {
            m_Position = pos;
            m_Direction = dir;
            m_fAngularVelocity = 0.0f;
            m_fRotation = 0.0f;
        }

        public Oak_Kinematic(Vector2 pos, Vector2 dir, float roation, float angularV)
        {
            m_Position = pos;
            m_Direction = dir;
            m_fRotation = roation;
            m_fAngularVelocity = angularV;
        }

        public void ZeroOut()
        {
            m_Position = Vector2.Zero;
            m_Direction = Vector2.Zero;
            m_fRotation = 0.0f;
            m_fAngularVelocity = 0.0f;
        }

        public void Update(float duration)
        {
            SetUpdate(duration, m_Direction, m_fAngularVelocity);
        }

        public void Update(Oak_Steering steer, float duration)
        {
            SetUpdate(duration, m_Direction, m_fAngularVelocity);
            m_Direction += steer.ForwardVector * duration;
            m_fAngularVelocity += steer.Rotation * duration;
        }

        public void Update(Oak_Steering steer, float drag, float duration)
        {
            SetUpdate(duration, m_Direction, m_fAngularVelocity);

            // SLow the direction and retoation vel
            drag = (float)Math.Pow(drag, duration);
            m_Direction *= drag;
            m_fAngularVelocity *= drag * drag;
            float xv = -20.0f;
            float xy = 20.0f;
            xv += xy;

            m_Direction += steer.ForwardVector * duration;
            m_fAngularVelocity += steer.Rotation * duration;
        }

        public void Update(Oak_Steering steer, Oak_Steering drag, float duration)
        {
            SetUpdate(duration, m_Direction, m_fAngularVelocity);

            m_Direction.X *= (float)Math.Pow(drag.ForwardVector.X, duration);
            m_Direction.Y *= (float)Math.Pow(drag.ForwardVector.Y, duration);
            m_fAngularVelocity *= (float)Math.Pow(drag.Rotation, duration);

            m_Direction += steer.ForwardVector * duration;
            m_fAngularVelocity += steer.Rotation * duration;

        }

        void SetUpdate(float duration, Vector2 dir, float rotation)
        {
            m_Position += dir * duration;
            m_fRotation += rotation * duration;
            m_fRotation = (float)Math.IEEERemainder(rotation, Math.PI * 2);
        }

        public void TrimSpeed(float speed)
        {
            float result = (m_Direction.X * m_Direction.X + m_Direction.Y * m_Direction.Y);
            if (result > speed * speed)
            {
                m_Direction = Vector2.Normalize(m_Direction);

                m_Direction *= speed;
            }


        }

        public void SetRotationFromVelocity()
        {
            float result = (m_Direction.X * m_Direction.X + m_Direction.Y * m_Direction.Y);

            if (result > 0)
                m_fRotation = (float)Math.Atan2(m_Direction.Y, m_Direction.X);
        }

        // Operator overloads

        public static bool operator ==(Oak_Kinematic lhs, Oak_Kinematic rhs)
        {
            return (lhs.m_Position == rhs.m_Position &&
                    lhs.m_Direction == rhs.m_Direction &&
                    lhs.m_fRotation == rhs.m_fRotation &&
                    lhs.m_fAngularVelocity == rhs.m_fAngularVelocity);
        }

        public static bool operator !=(Oak_Kinematic lhs, Oak_Kinematic rhs)
        {
            return (lhs.m_Position != rhs.m_Position ||
                    lhs.m_Direction != rhs.m_Direction ||
                    lhs.m_fRotation != rhs.m_fRotation ||
                    lhs.m_fAngularVelocity != rhs.m_fAngularVelocity);
        }

        public static bool operator <(Oak_Kinematic lhs, Oak_Kinematic rhs)
        {
            return (lhs.m_Position.X < rhs.m_Position.X);
        }

        public static bool operator >(Oak_Kinematic lhs, Oak_Kinematic rhs)
        {
            return (lhs.m_Position.X > rhs.m_Position.X);
        }

        public static Oak_Kinematic operator +(Oak_Kinematic lhs, Oak_Kinematic rhs)
        {
            Oak_Kinematic temp = new Oak_Kinematic();
            temp.m_Position = lhs.m_Position + rhs.m_Position;
            temp.m_Direction = lhs.m_Direction + rhs.m_Direction;
            temp.m_fAngularVelocity = lhs.m_fAngularVelocity + rhs.m_fAngularVelocity;
            temp.m_fRotation = lhs.m_fRotation + rhs.m_fRotation;
            return temp;
        }

        public static Oak_Kinematic operator -(Oak_Kinematic lhs, Oak_Kinematic rhs)
        {
            Oak_Kinematic temp = new Oak_Kinematic();
            temp.m_Position = lhs.m_Position - rhs.m_Position;
            temp.m_Direction = lhs.m_Direction - rhs.m_Direction;
            temp.m_fAngularVelocity = lhs.m_fAngularVelocity - rhs.m_fAngularVelocity;
            temp.m_fRotation = lhs.m_fRotation - rhs.m_fRotation;
            return temp;
        }

        public static Oak_Kinematic operator *(Oak_Kinematic lhs, Oak_Kinematic rhs)
        {
            Oak_Kinematic temp = new Oak_Kinematic();
            temp.m_Position = lhs.m_Position * rhs.m_Position;
            temp.m_Direction = lhs.m_Direction * rhs.m_Direction;
            temp.m_fAngularVelocity = lhs.m_fAngularVelocity * rhs.m_fAngularVelocity;
            temp.m_fRotation = lhs.m_fRotation * rhs.m_fRotation;
            return temp;
        }

    }
}
